#include <iostream>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <Eigen/Core>

using namespace cv;
using namespace std;

void findmatch(const Mat &image1, const Mat &image2, vector<KeyPoint> &keypoints1,
               vector<KeyPoint> &keypoints2, vector<DMatch> &goodmatches)
{
    Mat descriptors1, descriptors2;
    Ptr<FeatureDetector> detector=ORB::create();
    Ptr<DescriptorExtractor> descriptor=ORB::create();
    Ptr<DescriptorMatcher> matcher=DescriptorMatcher::create("BruteForce-Hamming");
    
    detector->detect(image1, keypoints1);
    detector->detect(image2, keypoints2);
    
    descriptor->compute(image1, keypoints1, descriptors1);
    descriptor->compute(image2, keypoints2, descriptors2);
    
    vector<DMatch> matches;
    matcher->match(keypoints1, keypoints2, matches);
    
    auto min_max=minmax_element(matches.begin(), matches.end(),[](DMatch &m1, DMatch &m2){
        return m1.distance>m2.distance;
    });
    
    double max_distance=min_max.first->distance;
    double min_distance=min_max.second->distance;
    
    for(int i=0 ;i<matches.size(); i++){
        if(matches[i].distance<max(2*min_distance, 30.0)){
            goodmatches.push_back(matches[i]);
        }
    }   
}

void getmatchpoints(const vector<KeyPoint> &keypoints1, const vector<KeyPoint> &keypoints2,
                    const vector<DMatch> &goodmatches, 
                    vector<Point2f> &points1, vector<Point2f> &points2)
{
    Point2f point1, point2;
    for(int i=0; i<goodmatches.size(); i++){
    point1.x=keypoints1[goodmatches[i].queryIdx].pt.x;
    point1.y=keypoints1[goodmatches[i].queryIdx].pt.y;
    point2.x=keypoints2[goodmatches[i].trainIdx].pt.x;
    point2.y=keypoints2[goodmatches[i].trainIdx].pt.y;
    points1.push_back(point1);
    points2.push_back(point2);
    }
}

void getcameraK(const double &focal_length, const double &alph,
                const double &imagewidth, const double &imageheight,
                Mat &K)
{
    K=(Mat_<double>(3,3)<<focal_length, 0, -imagewidth/2,
                            0, focal_length*alph, -imageheight/2,
                            0, 0, 1);
}

Mat getpost(const Mat &R, const Mat &t)
{
    Mat post;
    hconcat(R,t, post);
    return post;
}

void piex2rayplane(const Point2f &piexpoint, const Mat &K, Point2f &ppoint)
{
    ppoint.x=(piexpoint.x-K.at<double>(0,2))/K.at<double>(0,0);
    ppoint.y=(piexpoint.y-K.at<double>(1,2))/K.at<double>(1,1);
}

void piexl2rayplane(const vector<Point2f> &points1, const Mat &K, vector<Point2f> rpoints1)
{
    Point2f tempoint;
    for(int i=0; i<points1.size(); i++)
    {
        tempoint.x=(points1[i].x-K.at<double>(0,2))/K.at<double>(0,0);
        tempoint.y=(points1[i].y-K.at<double>(1,2))/K.at<double>(1,1);
        rpoints1.push_back(tempoint);
    }
}

void gettransmat(const vector<Point2f> &points1, const vector<Point2f> &points2,
                const Mat &K,
                Mat &Fun, Mat &Ess,
                Mat &R21, Mat &t21)
{
    Fun=findFundamentalMat(points1, points2, CV_FM_RANSAC);
    
    double focal_length=K.at<double>(0,0);
    Point2f pricipalpoint(-K.at<double>(0,2), -K.at<double>(1,2));
    Ess=findEssentialMat(points1, points2, focal_length, pricipalpoint);
    recoverPose(Ess, points1, points2, R21, t21, focal_length, pricipalpoint);
    
    Mat post10=Mat::eye(Size(4,4), CV_64F);
    Mat post21=getpost(R21, t21);
}

void get3dpoint(const Mat &R10, const Mat &t10, const Mat &R21, const Mat &t21,
                const vector<Point2f> &points1, const vector<Point2f> &points2,
                const Mat &K, vector<Point3f> &wpoint)
{
    vector<Point2f> rpoints1, rpoints2;
    piexl2rayplane(points1, K, rpoints1);
    piexl2rayplane(points2, K, rpoints2);
    
    Mat post10, post21;
    post10=getpost(R10, t10);
    post21=getpost(R21, t21);
    
    Mat ppoint;
    triangulatePoints(post10, post21, rpoints1, rpoints2, ppoint);
    for(int i=0; i<rpoints1.size(); i++){
        Mat rpoint=ppoint.col(i);
        rpoint/=rpoint.at<double>(3,0);
        Point3f point3d(rpoint.at<double>(0,0), rpoint.at<double>(1,0), rpoint.at<double>(2,0));
        wpoint.push_back(point3d);
    }
}

void getwpoints2(const vector<Point3f> &wpoints1, const Mat &post21, vector<Point3f> &wpoint2)
{
    Point3f wpoint;
    for(int i=0; i<wpoints1.size(); i++){
        wpoint.x=wpoints1[i].x*post21.at<double>(0,0)+
                 wpoints1[i].y*post21.at<double>(0,1)+
                 wpoints1[i].z*post21.at<double>(0,2)+
                 post21.at<double>(0,3);
        wpoint.y=wpoints1[i].x*post21.at<double>(1,0)+
                 wpoints1[i].y*post21.at<double>(1,1)+
                 wpoints1[i].z*post21.at<double>(1,2)+
                 post21.at<double>(1,3);
        wpoint.z=wpoints1[i].x*post21.at<double>(2,0)+
                 wpoints1[i].y*post21.at<double>(2,1)+
                 wpoints1[i].z*post21.at<double>(2,2)+
                 post21.at<double>(2,3);
        wpoint2.push_back(wpoint);
    }
}

void  postest(const vector<Point3f> &wpoint1, const vector<Point3f> &wpoint2, Mat &R, Mat &t)
{
    Point3f p1, p2;
    
    int N=wpoint1.size();
    for(int i=0;i<N;i++){
        p1+=wpoint1[i];
        p2+=wpoint2[i];
    }
    
    p1=Point3f(Vec3f(p1)/N);
    p2=Point3f(Vec3f(p2)/N);
    vector<Point3f> q1(N), q2(N);
    
    for(int i=0;i<N;++i){
        q1[i]=wpoint1[i]-p1;
        q2[i]=wpoint2[i]-p2;
    }
    
    Eigen::Matrix3d W=Eigen::Matrix3d::Zero();
    for(int i=0; i<N; ++i){
        W+=Eigen::Vector3f(q1[i].x, q1[i].y, q1[i].z)*Eigen::Vector3f(q2[i].x, q2[i].y, q2[i].z).transpose();
    }
    
    
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(W,Eigen::ComputerFullU|Eigen::ComputeFullV);
    Eigen::Matrix3d U=svd.matrixU();
    Eigen::Matrix3d V=svd.matrixV();
    
    Eigen::Matrix3d R_=U*(V.transpose());
    if(R_.determinant()<0){
        R_=-R_;
    }
    Eigen::Vector3d t_=Eigen::Vector3d(p1.x, p1.y, p1.z)-R_*Eigen::Vector3d(p2.x, p2.y, p2.z);
    
    
    R=(Mat_<double>(3,3)<<R_(0,0),R_(0,1),R_(0,2),
                          R_(1,0),R_(1,1),R_(1,2),
                          R_(2,0),R_(2,1),R_(2,2));
    t=(Mat_<double>(3,1)<<t_(0,0),t_(1,0),t_(2,0));
}
    


int main(int argc, char **argv) {
      Mat image1=imread("../1.jpg", IMREAD_ANYCOLOR);
    Mat image2=imread("../2.jpg", IMREAD_ANYCOLOR);
    assert(image1.data!=nullptr&&image2.data!=nullptr);
    
    double focallength, alph, imagewidth, imagehight;
    Mat K;
    focallength=1220.0;
    alph=1;
    imagewidth=1980;
    imagehight=1080;
    getcameraK(focallength, alph, imagewidth, imagehight,K);
    
    vector<KeyPoint> keypoints1, keypoints2;
    vector<DMatch> goodmatches;
    findmatch(image1, image2, keypoints1, keypoints2, goodmatches);
    cout<<goodmatches.size()<<endl;
    
    vector<Point2f> points1, points2;
    getmatchpoints(keypoints1, keypoints2, goodmatches, points1, points2);
    
    Mat Fun, Ess,  R10,t10, R21, t21;
    gettransmat(points1, points2, K, Fun, Ess, R21, t21);
    
    vector<Point3f> points3d1, points3d2;
    get3dpoint(R10, t10, R21, t21, points1, points2, K, points3d1);
    Mat post21=getpost(R21, t21);
    getwpoints2(points3d1, post21, points3d2);
    
    postest((points3d1, points3d2,R21,t21);
}
